#include "Marcos.h"
#include "Protocol.h"
#include "Transmiter.h"
#include "QP_Intf.h"

#include "spdlog/spdlog.h"
#include "Logger.h"
// using namespace NS_SDK;
// 使用示例






// int main() 
// {
//     float spd_get = 0;
//     bool flag_spd_succ = false;
//     bool flag_spd_set[6] = {0};
//     bool flag_spd_get[6] = {0};
//     bool ret = false;
//     int cnt_write = 0;
//     tpDNOriginEncoderStruct enc_pos;

//     tpDNPositionStruct tpDN_pos;
//     tpDNRouteStruct tpDN_route;
//     int index = 0;

//     TcpSocket Tcp_Ins; //创建socket并连接
//     Proto Proto_Ins(Tcp_Ins);

    

//     Proto_Ins.create_and_listen_socket(8080);

//     while(!Proto_Ins.Is_Socket_Connected())
//     {
//         std::this_thread::sleep_for(std::chrono::seconds(1));
//     }

//     while(!Proto_Ins.Is_All_Drv_Enable())
//     {
//         std::this_thread::sleep_for(std::chrono::milliseconds(100));
//     }

//     //----------以当前绝对值位置作为原点---------
//     Proto_Ins.SetOrigin(0, enc_pos);    //设置原点坐标点，并且存入文件

//     Proto_Ins.LoadOrigin();    //从文件加载原点坐标

//     Proto_Ins.GetOriginEncoder(enc_pos);  //获取原点坐标

//     //--------------------

//     Proto_Ins.Cmd_Set_Default_Spd(1,10.0f,&flag_spd_set[0]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Set_Default_Spd(2,10.0f,&flag_spd_set[1]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Set_Default_Spd(3,10.0f,&flag_spd_set[2]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Set_Default_Spd(4,10.0f,&flag_spd_set[3]);   // 10mm/秒

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Set_Default_Spd(5,120.0f,&flag_spd_set[4]);   //60度/秒

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Get_Default_Spd(1,&spd_get,&flag_spd_get[0]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Get_Default_Spd(2,&spd_get,&flag_spd_get[1]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Get_Default_Spd(3,&spd_get,&flag_spd_get[2]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Get_Default_Spd(4,&spd_get,&flag_spd_get[3]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     Proto_Ins.Cmd_Get_Default_Spd(5,&spd_get,&flag_spd_get[4]);

//     std::this_thread::sleep_for(std::chrono::milliseconds(100));

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);

//     std::cout << "速度设置: "<< flag_spd_set[0] <<" " << flag_spd_set[1] <<" "<< flag_spd_set[2] <<" "<< flag_spd_set[3] <<" "<<flag_spd_set[4] <<" "<<std::endl;
//     std::cout << "速度读取: "<< flag_spd_get[0] <<" " << flag_spd_get[1] <<" "<< flag_spd_get[2] <<" "<< flag_spd_get[3] <<" "<< flag_spd_get[4] <<" " <<std::endl;

//     // Proto_Ins.Cmd_Move_Sigle_Axis(5,-150.0f,-20.0f);  //移动单轴(轴5)，从当前位置向前移动360

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);


//     // Proto_Ins.Cmd_Move_Sigle_Axis(1,-10.0f,-1.0f);  //移动单轴(轴1)，从当前位置向前移动10000

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);


//     // Proto_Ins.Cmd_Move_Sigle_Axis(1,-10.0f,-1.0f);  //移动单轴(轴1)，从当前位置向前移动10000

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);

//     // Proto_Ins.Cmd_Move_Sigle_Axis(1,10000,300);  //移动单轴(轴1)，从当前位置向前移动10000
    
//     std::cout << "发送~~~~~~~20000! " <<std::endl;
//     ret = Proto_Ins.DBG_Virtual_Pts_Set_20000_WaitFin();   //发送1000个点到为止20000，阻塞式。

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);
//     //获取当前路劲点信息
//     Proto_Ins.Cmd_Get_CurPts_Infos(index, tpDN_route);

//     Proto_Ins.DBG_Virtual_Pts_Set_20000();  //下发一千个点位 从0到20000

//     Proto_Ins.Cmd_StartRun(0);    //启动运行

//     while(!Proto_Ins.Is_Motion_Fin())
//     {
//         std::this_thread::sleep_for(std::chrono::milliseconds(100));
//     }

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);

//     std::cout << "发送~~~~~~~0! " <<std::endl;
//     Proto_Ins.DBG_Virtual_Pts_Set_0();  //下发一千个点位 从0到20000

//     Proto_Ins.Cmd_StartRun(0);    //启动运行
//      std::this_thread::sleep_for(std::chrono::milliseconds(3000));
//     Proto_Ins.Cmd_PauseRun();   //暂停运行
//      std::this_thread::sleep_for(std::chrono::milliseconds(5000));
//     Proto_Ins.Cmd_ResumeRun();  //继续运行

//     while(!Proto_Ins.Is_Motion_Fin())
//     {
//         std::this_thread::sleep_for(std::chrono::milliseconds(100));
//     }

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);

//     std::cout << "发送~~~~~~~20000! " <<std::endl;
//     Proto_Ins.DBG_Virtual_Pts_Set_20000();  //下发一千个点位 从0到20000

//     Proto_Ins.Cmd_StartRun(0);    //启动运行

//     while(!Proto_Ins.Is_Motion_Fin())
//     {
//         std::this_thread::sleep_for(std::chrono::milliseconds(100));
//     }

//     //获取设备位置信息
//     Proto_Ins.Cmd_Get_Dev_Pos(tpDN_pos);

//     while(true)
//     {
//         std::this_thread::sleep_for(std::chrono::seconds(5));
//         Proto_Ins.Is_Motion_Fin();
//     }
    
//     #ifdef _WIN32
//         WSACleanup();
//     #endif

//     return 0;
// }

 void CallBack_MotorErr(uint16_t m1_err, uint16_t m2_err,uint16_t m3_err,uint16_t m4_err, uint16_t m5_err,uint16_t m6_err)
 {
    std::cout<< "--------------Motor Err Callback -------------"  <<std::endl;

    if(ERR_CANOPEN_TIMEOUT == m1_err)
    {
        std::cout<< "M1: Canopen TimeOut !" <<std::endl;
    }
    if(ERR_CANOPEN_TIMEOUT == m2_err)
    {
        std::cout<< "M2: Canopen TimeOut !" <<std::endl;
    }
    if(ERR_CANOPEN_TIMEOUT == m3_err)
    {
        std::cout<< "M3: Canopen TimeOut !" <<std::endl;
    }
    if(ERR_CANOPEN_TIMEOUT == m4_err)
    {
        std::cout<< "M4: Canopen TimeOut !" <<std::endl;
    }
    if(ERR_CANOPEN_TIMEOUT == m5_err)
    {
        std::cout<< "M5: Canopen TimeOut !" <<std::endl;
    }
    if(ERR_CANOPEN_TIMEOUT == m6_err)
    {
        std::cout<< "M6: Canopen TimeOut !" <<std::endl;
    }

    std::cout<< "M1Err: " << m1_err <<"M2Err: " << m2_err <<"M3Err: " << m3_err <<"M4Err: " << m4_err <<"M5Err: " << m5_err <<"M6Err: " << m6_err <<std::endl;
 }

 void CallBack_Trigger_Printf(int total, int index, int trigger_number,tpDNRouteStruct route)
 {
    std::cout<< "--------------Trigger Call -------------"  <<std::endl;
    std::cout<< " total: " << total<<" index: " <<index <<" trigger_number: "<<trigger_number <<std::endl;
    std::cout<< "x: " << route.pos.x <<" y: " <<route.pos.y <<" z: "<<route.pos.z <<std::endl;
    std::cout<< "rx: " << route.pos.rx <<" ry: " <<route.pos.ry <<" rz: "<<route.pos.rz <<std::endl;
 }



int main() 
{
    float spd_get = 0;
    bool flag_spd_succ = false;
    int flag_spd_set[6] = {0};
    int flag_spd_get[6] = {0};
    bool ret = false;
    int tp_ret = 0;
    int delay_cur_route = 0;

    int ReadPos_TimeOut = 0;

    tpDNPositionStruct tpDN_pos;
    tpDNRouteStruct tpDN_route;
    tpDNOriginEncoderStruct tmp_enc = {0};
    int index = 0;

    std::shared_ptr<spdlog::logger> log_man = LogMan::instance();  // 日志器成员变量

    TcpSocket Tcp_Ins; //创建socket并连接
    Proto Proto_Ins(Tcp_Ins);

    // spdlog::set_level(spdlog::level::debug);
    // spdlog::info("Welcome to spdlog! __INFO");
    // spdlog::warn("Welcome to spdlog! __WARN ");

    TP_DN_SetDebugLog(LOG_LVL_DEBUG);   //LOG_LVL_TRACE

    // log_man->trace("Welcome to spdlog! __TRACE");
    // log_man->debug("Welcome to spdlog! __DEBUG");
    // log_man->info("Welcome to spdlog! __INFO");
    // log_man->warn("Welcome to spdlog! __WARN");
    // log_man->error("Welcome to spdlog! __ERROR");
    // log_man->critical("Welcome to spdlog! __CRITICAL");

    log_man->info("SDK start.... ");

    TP_DN_Init();

    TP_DN_SetCallBack(CallBack_Trigger_Printf);    //设置回调函数

    TP_DN_SetCallBack_MotorErr(CallBack_MotorErr);    //设置电机故障回调函数
    

    while(!TP_Is_Connected())
    {
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    while(!TP_Is_AllDrv_Enable())
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    TP_DN_DO_Ccontrol(DO1,DO_ON); 
    TP_DN_DO_Ccontrol(DO1,DO_OFF); 
    TP_DN_DO_Ccontrol(DO2,DO_ON); 
    TP_DN_DO_Ccontrol(DO2,DO_OFF); 
    TP_DN_DO_Ccontrol(DO3,DO_ON); 
    TP_DN_DO_Ccontrol(DO3,DO_OFF); 
    TP_DN_DO_Ccontrol(DO4,DO_ON); 
    TP_DN_DO_Ccontrol(DO4,DO_OFF); 

    tp_ret = TP_DN_Refresh_Rt_Infos();

    // TP_DN_SetOrigin(0, tmp_enc);  //设置当前位置为原点
    // {
    //     std::cout<<" Load Origin points Failed !" <<std::endl;
    // }

    if(TP_DN_LoadOrigin() != TP_DN_STATUS_SUCCES) //加载原点坐标
    {
        std::cout<<" Load Origin points Failed !" <<std::endl;
    }


    TP_DN_GetOriginEncoder(tmp_enc);

    tp_ret = TP_DN_GetPosition(tpDN_pos);// 获取当前位置坐标 （加载坐标原点后一定要，刷新一下最新位置）

    // tmp_enc.ex -= 500;
    // tmp_enc.ey -= 500;
    // tmp_enc.ez_1 -= 500;
    // tmp_enc.ez_2 -= 500;
    // tmp_enc.erx -= 500;
    // tmp_enc.erz -=500;
    // TP_DN_SetOrigin(1, tmp_enc);  //设置指定位置为原点
    // // TP_DN_SetOrigin(0, tmp_enc);  //设置当前位置为原点
    // {
    //     std::cout<<" Load Origin points Failed !" <<std::endl;
    // }

    // if(TP_DN_LoadOrigin() != TP_DN_STATUS_SUCCES) //加载原点坐标
    // {
    //     std::cout<<" Load Origin points Failed !" <<std::endl;
    // }

    tp_ret = TP_DN_GetPosition(tpDN_pos); // 获取当前位置坐标 （设置完坐标原点后一定要刷新一下最新位置）

    // flag_spd_set[0] = TP_DN_SetDefaultSpeed(1,RATED_SPD_RPS_HC*ENC_PER_ROUND_HC);   //设置速度   HC 电机额定转速3000rpm
    // flag_spd_set[1] = TP_DN_SetDefaultSpeed(2,RATED_SPD_RPS_HC*ENC_PER_ROUND_HC);   //设置速度
    // flag_spd_set[2] = TP_DN_SetDefaultSpeed(3,RATED_SPD_RPS_HC*ENC_PER_ROUND_HC);   //设置速度
    // flag_spd_set[3] = TP_DN_SetDefaultSpeed(4,RATED_SPD_RPS_HC*ENC_PER_ROUND_HC);   //设置速度

    // flag_spd_get[0] = TP_DN_GetDefaultSpeed(1,spd_get);  //获取速度
    // flag_spd_get[1] = TP_DN_GetDefaultSpeed(2,spd_get);  //获取速度
    // flag_spd_get[2] = TP_DN_GetDefaultSpeed(3,spd_get);  //获取速度
    // flag_spd_get[3] = TP_DN_GetDefaultSpeed(4,spd_get);  //获取速度

    tp_ret = TP_DN_Refresh_Rt_Infos();
    tp_ret = TP_DN_GetPosition(tpDN_pos);

    // tp_ret = TP_DN_GetCurrentRoute(index,tpDN_route);   //
    std::cout << "Set Speed: "<< flag_spd_set[0] <<" " << flag_spd_set[1] <<" "<< flag_spd_set[2] <<" "<< flag_spd_set[3] <<" " <<std::endl;
    std::cout << "Read Speed: "<< flag_spd_get[0] <<" " << flag_spd_get[1] <<" "<< flag_spd_get[2] <<" "<< flag_spd_get[3] <<" " <<std::endl;


    // tp_ret = TP_DN_SingleMove(1,10.0f,1.0f);  //第1轴 往前10mm, 速度1mm/s
    // tp_ret = TP_DN_SingleMove(2,10.0f,2.0f);  //第2轴 往前10mm, 速度2mm/s
    // tp_ret = TP_DN_SingleMove(3,10.0f,4.0f);  //第3轴 往前10mm, 速度4mm/s
    // tp_ret = TP_DN_SingleMove(4,10.0f,6.0f);  //第4轴 往前10mm, 速度6mm/s
    // tp_ret = TP_DN_SingleMove(5,360.0f,36.0f);  //第5轴 往前360, 速度36度/s


    
    // std::cout << "Send routes points --------- turn Right! " <<std::endl;
    // tp_ret = TP_DN_GetPosition(tpDN_pos);   //先获取位置
    // DBG_Virtual_Pts_Set_Forword(tpDN_pos);  //用到了 TP_DN_SetPosition接口
    // while(!TP_DN_Routes_Finish()) //是否点位路径运行结束
    // {
    //     std::this_thread::sleep_for(std::chrono::milliseconds(100));
    // }


    // std::cout << "Send routes points --------- turn Left! " <<std::endl;
    // tp_ret = TP_DN_GetPosition(tpDN_pos);   //先获取位置
    // DBG_Virtual_Pts_Set_Backword(tpDN_pos);  //用到了 TP_DN_SetRoutes 接口 和TP_DN_StartRoutes 接口
    // while(!TP_DN_Routes_Finish()) //是否点位路径运行结束
    // {
    //     std::this_thread::sleep_for(std::chrono::milliseconds(100));
    // }

    // std::this_thread::sleep_for(std::chrono::seconds(2));



    while(TP_DN_GetPosition(tpDN_pos) != TP_DN_STATUS_SUCCES) //先获取位置
    {
        ReadPos_TimeOut++;
        std::cout<< "Read RT TimeOut: " << ReadPos_TimeOut<<std::endl;
    }

    // tp_ret = TP_DN_SingleMove(5,360.0f,6.0f);  //第5轴 往前360, 速度36度/s
    // tp_ret = TP_DN_SingleMove(6,360.0f,6.0f);  //第5轴 往前360, 速度36度/s


    // DBG_Virtual_Pts_Set_Forword(tpDN_pos);  //用到了 TP_DN_SetRoutes 接口 和TP_DN_StartRoutes 接口  
    // while(!TP_DN_Routes_Finish()) //是否点位路径运行结束
    // {
    //     std::this_thread::sleep_for(std::chrono::milliseconds(100));
    // }

    while(TP_DN_GetPosition(tpDN_pos) != TP_DN_STATUS_SUCCES) //先获取位置
    {
        ReadPos_TimeOut++;
        std::cout<< "Read RT TimeOut: " << ReadPos_TimeOut<<std::endl;
    }
    
    while(true)
    {
        std::this_thread::sleep_for(std::chrono::seconds(2));

        std::cout<< "-------------启动单轴运动 1------------------- " <<std::endl;
        // tp_ret = TP_DN_SingleMove(1,10.0f,1.0f);  //第1轴 往前10mm, 速度1mm/s
        // tp_ret = TP_DN_SingleMove(2,10.0f,2.0f);  //第2轴 往前10mm, 速度2mm/s
        do{
            tp_ret = TP_DN_SingleMove(3,10.0f,4.0f);
        }
        while(tp_ret != 0);
        // tp_ret = TP_DN_SingleMove(4,10.0f,6.0f);  //第4轴 往前10mm, 速度6mm/s
        // tp_ret = TP_DN_SingleMove(5,360.0f,36.0f);  //第5轴 往前360, 速度36度/s
        // tp_ret = TP_DN_SingleMove(6,360.0f,36.0f);  //第5轴 往前360, 速度36度/s

        std::this_thread::sleep_for(std::chrono::seconds(2));
        std::cout<< "-------------启动单轴运动 2------------------- " <<std::endl;
        // tp_ret = TP_DN_SingleMove(1,-10.0f,-1.0f);  //第1轴 往后10mm, 速度-1mm/s
        // tp_ret = TP_DN_SingleMove(2,-10.0f,-2.0f);  //第2轴 往后10mm, 速度-2mm/s
        do{
            tp_ret = TP_DN_SingleMove(3,-10.0f,-4.0f);  //第3轴 往后10mm, 速度-4mm/s
        }while(tp_ret != 0);
        // tp_ret = TP_DN_SingleMove(4,-10.0f,-6.0f);  //第4轴 往后10mm, 速度-6mm/s
        // tp_ret = TP_DN_SingleMove(5,-360.0f,-36.0f);  //第5轴 往后360, 速度-36度/s
        // tp_ret = TP_DN_SingleMove(6,-360.0f,-36.0f);  //第5轴 往后360, 速度-36度/s

      //-----------------后续暂时注释
        std::this_thread::sleep_for(std::chrono::seconds(2));

        while(TP_DN_GetPosition(tpDN_pos) != TP_DN_STATUS_SUCCES) //先获取位置
        {
            ReadPos_TimeOut++;
            std::cout<< "Read RT TimeOut: " << ReadPos_TimeOut<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }
        std::cout<< "-------------启动多轴运动 1------------------- " <<std::endl;
        std::cout << "*****xyz**curpos*****: "<< tpDN_pos.x <<", " << tpDN_pos.y <<", "<< tpDN_pos.z <<", "<< tpDN_pos.rx <<", "<<tpDN_pos.rz <<std::endl;
        // tp_ret = TP_DN_GetPosition(tpDN_pos);   //先获取位置
        DBG_Virtual_Pts_Set_Forward_WaitFin(tpDN_pos);  //用到了 TP_DN_SetPosition接口  （多轴同时往前）
        while(!TP_DN_Routes_Finish()) //是否点位路径运行结束
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        std::this_thread::sleep_for(std::chrono::seconds(2));

        while(TP_DN_GetPosition(tpDN_pos) != TP_DN_STATUS_SUCCES) //先获取位置
         {
            ReadPos_TimeOut++;
            std::cout<< "Read RT TimeOut: " << ReadPos_TimeOut<<std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }
        std::cout<< "-------------启动多轴运动 2------------------- " <<std::endl;
        std::cout << "*****xyz**curpos*****: "<< tpDN_pos.x <<", " << tpDN_pos.y <<", "<< tpDN_pos.z <<", "<< tpDN_pos.rx <<", "<<tpDN_pos.rz <<std::endl;
        // tp_ret = TP_DN_GetPosition(tpDN_pos);   //先获取位置
        DBG_Virtual_Pts_Set_Backword(tpDN_pos);  //用到了 TP_DN_SetRoutes 接口 和TP_DN_StartRoutes 接口  （多轴同时往后）
        while(!TP_DN_Routes_Finish()) //是否点位路径运行结束
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            if(++delay_cur_route >= 30)
            {
                delay_cur_route = 0;
                TP_DN_GetCurrentRoute(index, tpDN_route);
            }
            
        }
    }
    
    #ifdef _WIN32
        WSACleanup();
    #endif

    return 0;
}





